use r2r::{self, QosProfile};
use serde_json::json;
use std::sync::{Arc, Mutex};

const OBSTACLES_AVOID_API_ID: i64 = 1001;

pub struct ObsAvoidClient {
    publisher: Arc<Mutex<r2r::Publisher<r2r::unitree_api::msg::Request>>>,
    node: Arc<Mutex<r2r::Node>>,
}
// ros2 topic pub -r 10 \
//  --qos-reliability reliable \
// --qos-durability volatile --qos-depth 1 \
// /api/obstacles_avoid/request unitree_api/msg/Request \
//  "{header: {identity: {id: 1001, api_id: 1001}, \
// lease: {id: 0}, policy: {priority: 10, noreply: false}}, \
// parameter: \"{\\\"enable\\\": false}\"}"

impl ObsAvoidClient {
    pub fn from_node(node: &mut Arc<Mutex<r2r::Node>>) -> Result<Self, r2r::Error> {
        let publisher = node
            .lock()
            .unwrap()
            .create_publisher::<r2r::unitree_api::msg::Request>(
                "/api/obstacles_avoid/request",
                QosProfile::default(),
            )?;
        Ok(Self {
            publisher: Arc::new(Mutex::new(publisher)),
            node: node.clone(),
        })
    }

    pub fn new(node_name: &str, namespace: &str) -> Result<Self, r2r::Error> {
let ctx = r2r::Context::create()?;
        let node = r2r::Node::create(ctx, node_name, namespace)?;

        let node_arc = Arc::new(Mutex::new(node));

        // Create publisher for lidar control topic
        let publisher = node_arc
            .lock()
            .unwrap()
            .create_publisher::<r2r::unitree_api::msg::Request>("/api/obstacles_avoid/request", QosProfile::default())?;

        Ok(Self {
            publisher: Arc::new(Mutex::new(publisher)),
            node: node_arc.clone(),
        })
    }

    pub fn set_enable(&self, req: &mut r2r::unitree_api::msg::Request, enable: bool) {
        req.header.identity.api_id = OBSTACLES_AVOID_API_ID;
        req.parameter = json!({ "enable": enable }).to_string();
        let _ = self.publisher.lock().unwrap().publish(req);
    }
}
